import torch
import torch.nn as nn
import torch.nn.functional as F
from network.Pixor_parts import Basic_Block, inconv, up
try:
    from torch.hub import load_state_dict_from_url
except ImportError:
    from torch.utils.model_zoo import load_url as load_state_dict_from_url


def mask_points(xyz, depth_index, batch_idx):
    """
    filter points
    :param xyz: batch, points, 3
    :param depth_index: batch, points, 2
    :return:
    """
    mask1 = (xyz[batch_idx, :, 0] >= -36) * (xyz[batch_idx, :, 0] < 36) * (xyz[batch_idx, :, 2] > 0) * (
                xyz[batch_idx, :, 2] < 66) * (
                    xyz[batch_idx, :, 1] >= -1) * (xyz[batch_idx, :, 1] < 2.5)

    xyz_mask1 = xyz[batch_idx][mask1]
    depth_index_mask1 = depth_index[batch_idx][mask1]
    xz_mask1 = xyz_mask1[:, (0, 2)]
    xz_mask1[:, 0] += 40
    xz_mask1_quant = torch.floor(xz_mask1 * 10)
    return xz_mask1_quant.long(), depth_index_mask1.long()

class PixorNet_Fusion(nn.Module):
    def __init__(self, n_channels, groupnorm=False, resnet_type='resnet50',
                 resnet_pretrained=True, resnet_chls=64, image_downscale=1):

        self.image_downscale = image_downscale
        super(PixorNet_Fusion, self).__init__()
        block = Basic_Block
        self.inplanes = 32

        self.inc = inconv(n_channels, 32)
        self.down1 = self._make_layer(block, [24, 24, 96], stride=2)
        self.down2 = self._make_layer(
            block, [48, 48, 48, 48, 192, 192], stride=2)

        self.inplanes += resnet_chls
        self.down3 = self._make_layer(
            block, [64, 64, 64, 64, 256, 256], stride=2, groupnorm=groupnorm)
        self.inplanes += resnet_chls

        self.down4 = self._make_layer(block, [96, 96, 384], stride=2, groupnorm=groupnorm)
        self.inplanes += resnet_chls

        self.bridge = nn.Sequential(
            nn.Conv2d(self.inplanes, 192, 1),
            nn.BatchNorm2d(192) if not groupnorm else nn.GroupNorm(8, 192),
            nn.ReLU(inplace=True),
        )

        self.up1 = up(192, 256+resnet_chls, 128, groupnorm=groupnorm)
        self.up2 = up(128, 192+resnet_chls, 96, groupnorm=groupnorm)

        self.shared_header = nn.Sequential(
            nn.Conv2d(96, 96, 3, stride=1, padding=1),
            nn.BatchNorm2d(96) if not groupnorm else nn.GroupNorm(8, 96),
            nn.ReLU(inplace=True),
            nn.Conv2d(96, 96, 3, stride=1, padding=1),
            nn.BatchNorm2d(96) if not groupnorm else nn.GroupNorm(8, 96),
            nn.ReLU(inplace=True),
            nn.Conv2d(96, 96, 3, stride=1, padding=1),
            nn.BatchNorm2d(96) if not groupnorm else nn.GroupNorm(8, 96),
            nn.ReLU(inplace=True),
            nn.Conv2d(96, 96, 3, stride=1, padding=1),
            nn.BatchNorm2d(96) if not groupnorm else nn.GroupNorm(8, 96),
            nn.ReLU(inplace=True)
        )

        self.classification_header = nn.Sequential(
            nn.Conv2d(96, 1, 3, stride=1, padding=1),
        )
        self.regression_header = nn.Conv2d(96, 6, 3, stride=1, padding=1)

        for m in self.modules():
            if isinstance(m, nn.Conv2d):
                nn.init.kaiming_normal_(
                    m.weight, mode='fan_out', nonlinearity='relu')
            elif isinstance(m, nn.BatchNorm2d) or isinstance(m, nn.GroupNorm):
                nn.init.constant_(m.weight, 1)
                nn.init.constant_(m.bias, 0)

        if resnet_type == 'resnet50':
            self.resnet = resnet50(resnet_pretrained, resnet_chls=resnet_chls)
        else:
            self.resnet = resnet18(resnet_pretrained, resnet_chls=resnet_chls)

    def _make_layer(self, block, planes, stride=1, groupnorm=False):
        layers = []
        for i in range(len(planes)):
            downsample = None
            if stride != 1 or self.inplanes != planes[i] * block.expansion:
                downsample = nn.Sequential(
                    nn.Conv2d(self.inplanes, planes[i] * block.expansion,
                              kernel_size=1, stride=1, bias=False),
                    nn.MaxPool2d(kernel_size=stride, stride=stride,
                                 ceil_mode=True),  # Harry
                    nn.BatchNorm2d(planes[i] * block.expansion) if not groupnorm else nn.GroupNorm(8, planes[i] * block.expansion),
                    # nn.GroupNorm(1, planes[i] * block.expansion),
                )

            layers.append(block(self.inplanes, planes[i], stride, downsample, groupnorm))
            stride = 1
            self.inplanes = planes[i] * block.expansion

        return nn.Sequential(*layers)

    def forward(self, x, img, img_index, bev_index, return_intermediates=False):
        # transform[0] bev index, transform[1] image index

        # resnet features
        res4x, res8x, res16x = self.resnet(img)
        # down scale
        bev_down = 1
        img_down = self.image_downscale

        x1 = self.inc(x)
        x2 = self.down1(x1)
        x3 = self.down2(x2)
        bev_down = bev_down * 4

        # precompute mapping
        bev_list, img_list = [], []
        for i in range(x3.shape[0]):
            bev_mask, img_mask = mask_points(bev_index, img_index, i)
            bev_list.append(bev_mask)
            img_list.append(img_mask)

        x3_img = torch.zeros((x3.shape[0], res4x.shape[1], x3.shape[2], x3.shape[3]), device='cuda')
        img_down = img_down * 4
        # TODO: remove forloop in the batch dim
        for i in range(x3.shape[0]):
            x3_img[i, :, bev_list[i][:,1]//bev_down, bev_list[i][:,0]//bev_down] = res4x[i, :, img_list[i][:,1]//img_down, img_list[i][:,0]//img_down]

        x3 = torch.cat([x3,x3_img], dim=1)

        x4 = self.down3(x3)

        x4_img = torch.zeros((x4.shape[0], res8x.shape[1], x4.shape[2], x4.shape[3]), device='cuda')
        bev_down = bev_down * 2
        img_down = img_down * 2
        for i in range(x3.shape[0]):
            x4_img[i, :, bev_list[i][:,1]//bev_down, bev_list[i][:,0]//bev_down] = res8x[i, :, img_list[i][:,1]//img_down, img_list[i][:,0]//img_down]

        x4 = torch.cat([x4, x4_img], dim=1)

        x5 = self.down4(x4)

        x5_img = torch.zeros((x5.shape[0], res16x.shape[1], x5.shape[2], x5.shape[3]), device='cuda')
        bev_down = bev_down * 2
        img_down = img_down * 2
        for i in range(x3.shape[0]):
            x5_img[i, :, bev_list[i][:,1]//bev_down, bev_list[i][:,0]//bev_down] = res16x[i, :, img_list[i][:,1]//img_down, img_list[i][:,0]//img_down]

        x5 = torch.cat([x5, x5_img], dim=1)

        x6 = self.bridge(x5)

        x7 = self.up1(x6, x4)
        x8 = self.up2(x7, x3)
        x9 = self.shared_header(x8)
        logits = self.classification_header(x9)
        return torch.sigmoid(logits), self.regression_header(x9)





model_urls = {
    'resnet18': 'https://download.pytorch.org/models/resnet18-5c106cde.pth',
    'resnet34': 'https://download.pytorch.org/models/resnet34-333f7ec4.pth',
    'resnet50': 'https://download.pytorch.org/models/resnet50-19c8e357.pth',
    'resnet101': 'https://download.pytorch.org/models/resnet101-5d3b4d8f.pth',
    'resnet152': 'https://download.pytorch.org/models/resnet152-b121ed2d.pth',
}


def conv3x3(in_planes, out_planes, stride=1, groups=1, dilation=1):
    """3x3 convolution with padding"""
    return nn.Conv2d(in_planes, out_planes, kernel_size=3, stride=stride,
                     padding=dilation, groups=groups, bias=False, dilation=dilation)


def conv1x1(in_planes, out_planes, stride=1):
    """1x1 convolution"""
    return nn.Conv2d(in_planes, out_planes, kernel_size=1, stride=stride, bias=False)


class BasicBlock(nn.Module):
    expansion = 1

    def __init__(self, inplanes, planes, stride=1, downsample=None, groups=1,
                 base_width=64, dilation=1, norm_layer=None):
        super(BasicBlock, self).__init__()
        if norm_layer is None:
            norm_layer = nn.BatchNorm2d
        if groups != 1 or base_width != 64:
            raise ValueError('BasicBlock only supports groups=1 and base_width=64')
        if dilation > 1:
            raise NotImplementedError("Dilation > 1 not supported in BasicBlock")
        # Both self.conv1 and self.downsample layers downsample the input when stride != 1
        self.conv1 = conv3x3(inplanes, planes, stride)
        self.bn1 = norm_layer(planes)
        self.relu = nn.ReLU(inplace=True)
        self.conv2 = conv3x3(planes, planes)
        self.bn2 = norm_layer(planes)
        self.downsample = downsample
        self.stride = stride

    def forward(self, x):
        identity = x

        out = self.conv1(x)
        out = self.bn1(out)
        out = self.relu(out)

        out = self.conv2(out)
        out = self.bn2(out)

        if self.downsample is not None:
            identity = self.downsample(x)

        out += identity
        out = self.relu(out)

        return out


class Bottleneck(nn.Module):
    expansion = 4

    def __init__(self, inplanes, planes, stride=1, downsample=None, groups=1,
                 base_width=64, dilation=1, norm_layer=None):
        super(Bottleneck, self).__init__()
        if norm_layer is None:
            norm_layer = nn.BatchNorm2d
        width = int(planes * (base_width / 64.)) * groups
        # Both self.conv2 and self.downsample layers downsample the input when stride != 1
        self.conv1 = conv1x1(inplanes, width)
        self.bn1 = norm_layer(width)
        self.conv2 = conv3x3(width, width, stride, groups, dilation)
        self.bn2 = norm_layer(width)
        self.conv3 = conv1x1(width, planes * self.expansion)
        self.bn3 = norm_layer(planes * self.expansion)
        self.relu = nn.ReLU(inplace=True)
        self.downsample = downsample
        self.stride = stride

    def forward(self, x):
        identity = x

        out = self.conv1(x)
        out = self.bn1(out)
        out = self.relu(out)

        out = self.conv2(out)
        out = self.bn2(out)
        out = self.relu(out)

        out = self.conv3(out)
        out = self.bn3(out)

        if self.downsample is not None:
            identity = self.downsample(x)

        out += identity
        out = self.relu(out)

        return out


class ResNet(nn.Module):

    def __init__(self, block, layers, num_classes=1000, zero_init_residual=False,
                 groups=1, width_per_group=64, replace_stride_with_dilation=None,
                 norm_layer=None, resnet_chls=64):
        super(ResNet, self).__init__()
        if norm_layer is None:
            norm_layer = nn.BatchNorm2d
        self._norm_layer = norm_layer

        self.inplanes = 64
        self.dilation = 1
        if replace_stride_with_dilation is None:
            # each element in the tuple indicates if we should replace
            # the 2x2 stride with a dilated convolution instead
            replace_stride_with_dilation = [False, False, False]
        if len(replace_stride_with_dilation) != 3:
            raise ValueError("replace_stride_with_dilation should be None "
                             "or a 3-element tuple, got {}".format(replace_stride_with_dilation))
        self.groups = groups
        self.base_width = width_per_group
        self.conv1 = nn.Conv2d(3, self.inplanes, kernel_size=7, stride=2, padding=3,
                               bias=False)
        self.bn1 = norm_layer(self.inplanes)
        self.relu = nn.ReLU(inplace=True)
        self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1)
        self.layer1 = self._make_layer(block, 64, layers[0])

        self.layer2 = self._make_layer(block, 128, layers[1], stride=2,
                                       dilate=replace_stride_with_dilation[0])
        self.layer3 = self._make_layer(block, 256, layers[2], stride=2,
                                       dilate=replace_stride_with_dilation[1])
        self.layer1_1x1 = nn.Conv2d(64*block.expansion, resnet_chls, 1)
        self.layer2_1x1 = nn.Conv2d(128*block.expansion, resnet_chls, 1)
        self.layer3_1x1 = nn.Conv2d(256*block.expansion, resnet_chls, 1)

        for m in self.modules():
            if isinstance(m, nn.Conv2d):
                nn.init.kaiming_normal_(m.weight, mode='fan_out', nonlinearity='relu')
            elif isinstance(m, (nn.BatchNorm2d, nn.GroupNorm)):
                nn.init.constant_(m.weight, 1)
                nn.init.constant_(m.bias, 0)

        # Zero-initialize the last BN in each residual branch,
        # so that the residual branch starts with zeros, and each residual block behaves like an identity.
        # This improves the model by 0.2~0.3% according to https://arxiv.org/abs/1706.02677
        if zero_init_residual:
            for m in self.modules():
                if isinstance(m, Bottleneck):
                    nn.init.constant_(m.bn3.weight, 0)
                elif isinstance(m, BasicBlock):
                    nn.init.constant_(m.bn2.weight, 0)

    def _make_layer(self, block, planes, blocks, stride=1, dilate=False):
        norm_layer = self._norm_layer
        downsample = None
        previous_dilation = self.dilation
        if dilate:
            self.dilation *= stride
            stride = 1
        if stride != 1 or self.inplanes != planes * block.expansion:
            downsample = nn.Sequential(
                conv1x1(self.inplanes, planes * block.expansion, stride),
                norm_layer(planes * block.expansion),
            )

        layers = []
        layers.append(block(self.inplanes, planes, stride, downsample, self.groups,
                            self.base_width, previous_dilation, norm_layer))
        self.inplanes = planes * block.expansion
        for _ in range(1, blocks):
            layers.append(block(self.inplanes, planes, groups=self.groups,
                                base_width=self.base_width, dilation=self.dilation,
                                norm_layer=norm_layer))

        return nn.Sequential(*layers)

    def forward(self, x):
        x = self.conv1(x)
        x = self.bn1(x)
        x = self.relu(x)
        x = self.maxpool(x)

        x1 = self.layer1(x)
        x2 = self.layer2(x1)
        x3 = self.layer3(x2)
        x1 = self.layer1_1x1(x1)
        x2 = self.layer2_1x1(x2)
        x3 = self.layer3_1x1(x3)


        return x1,x2,x3


def _resnet(arch, inplanes, planes, pretrained, progress, resnet_chls, **kwargs):
    model = ResNet(inplanes, planes, resnet_chls=resnet_chls, **kwargs)
    if pretrained:
        model_dict = model.state_dict()
        state_dict = load_state_dict_from_url(model_urls[arch],
                                              progress=progress)
        params = {k: v for k, v in state_dict.items() if k in model_dict}
        model_dict.update(params)
        model.load_state_dict(model_dict)
    return model


def resnet18(pretrained=False, progress=True, resnet_chls=64, **kwargs):
    """Constructs a ResNet-18 model.

    Args:
        pretrained (bool): If True, returns a model pre-trained on ImageNet
        progress (bool): If True, displays a progress bar of the download to stderr
    """
    return _resnet('resnet18', BasicBlock, [2, 2, 2, 2], pretrained, progress, resnet_chls=resnet_chls,
                   **kwargs)


def resnet34(pretrained=False, progress=True, resnet_chls=64, **kwargs):
    """Constructs a ResNet-34 model.

    Args:
        pretrained (bool): If True, returns a model pre-trained on ImageNet
        progress (bool): If True, displays a progress bar of the download to stderr
    """
    return _resnet('resnet34', BasicBlock, [3, 4, 6, 3], pretrained, progress, resnet_chls=resnet_chls,
                   **kwargs)


def resnet50(pretrained=False, progress=True, resnet_chls=64, **kwargs):
    """Constructs a ResNet-50 model.

    Args:
        pretrained (bool): If True, returns a model pre-trained on ImageNet
        progress (bool): If True, displays a progress bar of the download to stderr
    """
    return _resnet('resnet50', Bottleneck, [3, 4, 6, 3], pretrained, progress, resnet_chls=resnet_chls,
                   **kwargs)


